{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# BetweenFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`BetweenFactor<VALUE>` is a fundamental factor in GTSAM used to represent measurements of the relative transformation (motion) between two variables of the same type `VALUE`.\n",
        "Common examples include:\n",
        "*   `BetweenFactorPose2`: Represents odometry measurements between 2D robot poses.\n",
        "*   `BetweenFactorPose3`: Represents odometry or relative pose estimates between 3D poses.\n",
        "\n",
        "The `VALUE` type must be a Lie Group (e.g., `Pose2`, `Pose3`, `Rot2`, `Rot3`).\n",
        "\n",
        "The factor encodes a constraint based on the measurement `measured_`, such that the error is calculated based on the difference between the predicted relative transformation and the measured one. Specifically, if the factor connects keys $k_1$ and $k_2$ corresponding to values $X_1$ and $X_2$, and the measurement is $Z$, the factor aims to minimize:\n",
        "\n",
        "$$ \\text{error}(X_1, X_2) = \\text{Log}(Z^{-1} \\cdot (X_1^{-1} \\cdot X_2)) $$\n",
        "\n",
        "where `Log` is the logarithmic map for the Lie group `VALUE`, $X_1^{-1} \\cdot X_2$ is the predicted relative transformation `between(X1, X2)`, and $Z^{-1}$ is the inverse of the measurement. The error vector lives in the tangent space of the identity element of the group.\n",
        "\n",
        "`BetweenConstraint<VALUE>` is a derived class that uses a `noiseModel::Constrained` noise model, effectively enforcing the relative transformation to be exactly the measured value."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/BetweenFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import BetweenFactorPose2, BetweenFactorPose3\n",
        "from gtsam import Pose2, Pose3, Rot3, Point3\n",
        "from gtsam import NonlinearFactorGraph, Values\n",
        "from gtsam import symbol_shorthand\n",
        "import graphviz\n",
        "\n",
        "X = symbol_shorthand.X"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating a BetweenFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "You create a `BetweenFactor` by specifying:\n",
        "1.  The keys of the two variables it connects (e.g., `X(0)`, `X(1)`).\n",
        "2.  The measured relative transformation (e.g., a `Pose2` or `Pose3`).\n",
        "3.  A noise model describing the uncertainty of the measurement."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "BetweenFactorPose2: BetweenFactor(x0,x1)\n",
            "  measured:  (1, 0, 0)\n",
            "  noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
            "\n",
            "BetweenFactorPose3: BetweenFactor(x1,x2)\n",
            "  measured:  R: [\n",
            "\t0.995004165, -0.0998334166, 0;\n",
            "\t0.0998334166, 0.995004165, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0.5   0   0\n",
            "  noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.1; 0.1; 0.1];\n"
          ]
        }
      ],
      "source": [
        "# Example for Pose2 (2D SLAM odometry)\n",
        "key1 = X(0)\n",
        "key2 = X(1)\n",
        "measured_pose2 = Pose2(1.0, 0.0, 0.0) # Move 1 meter forward\n",
        "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
        "\n",
        "between_factor_pose2 = BetweenFactorPose2(key1, key2, measured_pose2, odometry_noise)\n",
        "between_factor_pose2.print(\"BetweenFactorPose2: \")\n",
        "\n",
        "# Example for Pose3 (3D SLAM odometry)\n",
        "measured_pose3 = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0)) # Move 0.5m forward, yaw 0.1 rad\n",
        "odometry_noise_3d = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05, 0.1, 0.1, 0.1]))\n",
        "\n",
        "between_factor_pose3 = BetweenFactorPose3(X(1), X(2), measured_pose3, odometry_noise_3d)\n",
        "between_factor_pose3.print(\"\\nBetweenFactorPose3: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The `.error(values)` method calculates the error vector based on the current estimates of the variables in the `Values` object."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Error vector for BetweenFactorPose2: 0.3750000000000002\n",
            "Manual unwhitened error calculation: [0.10247917 0.09747917 0.05      ]\n",
            "Factor unwhitened error: [0.1  0.1  0.05]\n",
            "Manually whitened error: [0.51239583 0.48739583 0.5       ]\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "values.insert(X(0), Pose2(0.0, 0.0, 0.0))\n",
        "values.insert(X(1), Pose2(1.1, 0.1, 0.05)) # Slightly off from measurement\n",
        "\n",
        "# Evaluate the error for the Pose2 factor\n",
        "error_vector = between_factor_pose2.error(values)\n",
        "print(f\"Error vector for BetweenFactorPose2: {error_vector}\")\n",
        "\n",
        "# The unwhitened error is Log(Z^-1 * (X1^-1 * X2))\n",
        "pose0 = values.atPose2(X(0))\n",
        "pose1 = values.atPose2(X(1))\n",
        "predicted_relative = pose0.between(pose1)\n",
        "error_pose = measured_pose2.inverse() * predicted_relative\n",
        "unwhitened_error_expected = Pose2.Logmap(error_pose)\n",
        "print(f\"Manual unwhitened error calculation: {unwhitened_error_expected}\")\n",
        "print(f\"Factor unwhitened error: {between_factor_pose2.unwhitenedError(values)}\")\n",
        "\n",
        "# The whitened error (returned by error()) applies the noise model\n",
        "# For diagonal noise model, error_vector = unwhitened_error / sigmas\n",
        "sigmas = odometry_noise.sigmas()\n",
        "whitened_expected = unwhitened_error_expected / sigmas\n",
        "print(f\"Manually whitened error: {whitened_expected}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "viz_header_md"
      },
      "source": [
        "## Visualization"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "viz_example_code"
      },
      "outputs": [
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 2.50.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"206pt\" height=\"84pt\"\n",
              " viewBox=\"0.00 0.00 206.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
              "<polygon fill=\"white\" stroke=\"transparent\" points=\"-4,4 -4,-79.6 202,-79.6 202,4 -4,4\"/>\n",
              "<!-- var8646911284551352320 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "<!-- factor0 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>factor0</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"75\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M40.37,-41.61C52.8,-27.68 69.99,-8.41 74.09,-3.81\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x1</text>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor0 -->\n",
              "<g id=\"edge2\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M91.67,-40.17C85.49,-26.32 77.36,-8.1 75.43,-3.76\"/>\n",
              "</g>\n",
              "<!-- factor1 -->\n",
              "<g id=\"node5\" class=\"node\">\n",
              "<title>factor1</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"122\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor1 -->\n",
              "<g id=\"edge3\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M106.03,-40.17C111.94,-26.32 119.74,-8.1 121.59,-3.76\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352322 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>var8646911284551352322</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x2</text>\n",
              "</g>\n",
              "<!-- var8646911284551352322&#45;&#45;factor1 -->\n",
              "<g id=\"edge4\" class=\"edge\">\n",
              "<title>var8646911284551352322&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M157.61,-41.9C144.92,-27.96 127.17,-8.48 122.94,-3.83\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x2622f1f9fd0>"
            ]
          },
          "execution_count": 5,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "graph = NonlinearFactorGraph()\n",
        "graph.add(between_factor_pose2)\n",
        "graph.add(between_factor_pose3)\n",
        "\n",
        "dot_string = graph.dot(values)\n",
        "graphviz.Source(dot_string)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
